#include "SF_IMU.h"
#include "esp_system.h"
#include <Arduino.h>
#include <ArduinoJson.h>
#include <HardwareSerial.h>
#include <SimpleFOC.h>
#include <Wire.h>
#include <WiFi.h>
#include <ESPAsyncWebServer.h>
#include <AsyncTCP.h>
#include "index_html.h"
// 添加用于内存调试的头文件
#include "esp_heap_caps.h"

// WiFi设置
const char* ssid = "BC_Car_AP";      // WiFi接入点名称
const char* password = "12345678";   // WiFi密码（至少8个字符）

// 创建AsyncWebServer对象
AsyncWebServer server(80);

// 速度控制结构体
struct speed_control {
  float forward; // 前进速度
  float turn;    // 转向速度
  unsigned long last_update_time; // 最后更新时间
  bool data_processed; // 数据是否已处理标志
};

speed_control speedControl = {0.0, 0.0, 0, true}; // 初始化速度控制参数，默认已处理

// 定时器相关变量
unsigned long last_print_time = 0;
const unsigned long print_interval = 2000; // 1秒 = 1000毫秒

// 定义AS5600磁编码器
MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C);
MagneticSensorI2C sensor2 = MagneticSensorI2C(AS5600_I2C);

// BLDC电机和驱动实例
BLDCMotor motor1 = BLDCMotor(7);
BLDCDriver3PWM driver1 = BLDCDriver3PWM(4, 5, 6, 7);

BLDCMotor motor2 = BLDCMotor(7);
BLDCDriver3PWM driver2 = BLDCDriver3PWM(35, 36, 37, 38);

// 创建IMU实例
SF_IMU imu(Wire);

// 定义角度和速度PID控制器
PIDController pid_stb = PIDController(0.37, 0, 0.005, 100000, 10);
PIDController pid_vel = PIDController(0.1, 0, 0.005, 10000, 10);

// 电机角度和转速
float motor1AngleDeg, motor2AngleDeg;
float motor1SpeedRPM, motor2SpeedRPM;
float targetangle = 0.0; // 添加全局变量targetangle

// 前向声明所有函数原型
void print_sensor_data();

// 添加全局变量来跟踪串口状态
bool serialConnected = false;

// 处理传感器数据请求
void handleSensorData(AsyncWebServerRequest *request) {
  // 使用静态JSON文档，避免堆内存分配
  StaticJsonDocument<256> doc;
  
  // 添加数据有效性检查并限制小数位数
  // 角度数据
  doc["roll"] = isnan(imu.angle[0]) ? 0.0f : roundf(imu.angle[0] * 10) / 10;
  doc["pitch"] = isnan(imu.angle[1]) ? 0.0f : roundf(imu.angle[1] * 10) / 10;
  doc["yaw"] = isnan(imu.angle[2]) ? 0.0f : roundf(imu.angle[2] * 10) / 10;
  
  // 温度数据
  doc["temp"] = isnan(imu.temp) ? 25.0f : roundf(imu.temp * 10) / 10;
  
  // 电机数据
  doc["motor1Angle"] = isnan(motor1AngleDeg) ? 0.0f : roundf(motor1AngleDeg * 10) / 10;
  doc["motor1Speed"] = isnan(motor1SpeedRPM) ? 0.0f : roundf(motor1SpeedRPM * 10) / 10;
  doc["motor2Angle"] = isnan(motor2AngleDeg) ? 0.0f : roundf(motor2AngleDeg * 10) / 10;
  doc["motor2Speed"] = isnan(motor2SpeedRPM) ? 0.0f : roundf(motor2SpeedRPM * 10) / 10;
  
  // 目标角度
  doc["targetAngle"] = isnan(targetangle) ? 0.0f : roundf(targetangle * 10) / 10;
  
  // 使用静态缓冲区进行序列化
  static char jsonBuffer[256];
  size_t len = serializeJson(doc, jsonBuffer, sizeof(jsonBuffer));
  
  // 确保序列化成功
  if (len > 0 && len < sizeof(jsonBuffer)) {
    request->send(200, "application/json", jsonBuffer);
  } else {
    // 序列化失败或缓冲区溢出
    request->send(500, "application/json", "{\"error\":\"序列化失败\"}");
  }
}

// 处理控制命令
void handleControl(AsyncWebServerRequest *request) {
  // 这个函数只处理非POST请求，实际控制命令在handleBody中处理
  request->send(405, "text/plain", "Method Not Allowed");
}

// 处理POST请求体
void handleBody(AsyncWebServerRequest *request, uint8_t *data, size_t len, size_t index, size_t total) {
  if (index + len != total) {
    // 如果数据不完整，等待更多数据
    return;
  }
  
  // 检查数据长度是否合理
  if (len > 512) {
    request->send(400, "text/plain", "数据过大");
    return;
  }
  
  // 使用静态缓冲区，避免动态内存分配
  static char buffer[512];
  if (len >= sizeof(buffer)) {
    request->send(400, "text/plain", "数据过大");
    return;
  }
  
  // 复制数据到缓冲区并确保以NULL结尾
  memcpy(buffer, data, len);
  buffer[len] = '\0';
  
  // 使用StaticJsonDocument避免堆内存分配
  StaticJsonDocument<256> doc;
  DeserializationError error = deserializeJson(doc, buffer);
  
  if (error) {
    request->send(400, "text/plain", "无效的JSON数据");
    return;
  }
  
  // 验证JSON数据中是否包含必要字段
  if (!doc.containsKey("turn") || !doc.containsKey("forward")) {
    request->send(400, "text/plain", "缺少必要的控制参数");
    return;
  }
  
  // 验证数据类型
  if (!doc["turn"].is<float>() && !doc["turn"].is<int>() || 
      !doc["forward"].is<float>() && !doc["forward"].is<int>()) {
    request->send(400, "text/plain", "控制参数类型错误");
    return;
  }
  
  float turnValue = doc["turn"].as<float>();
  float forwardValue = doc["forward"].as<float>();
  
  // 范围检查
  if (turnValue >= -1.0 && turnValue <= 1.0 && 
      forwardValue >= -1.0 && forwardValue <= 1.0) {
    
    // 检查是否为零值状态
    bool isZeroState = (abs(turnValue) < 0.01f && abs(forwardValue) < 0.01f);
    
    // 检查数值是否有明显变化（大于0.01）或者是零值状态
    bool hasChanged = (abs(speedControl.turn - turnValue) > 0.01) || 
                       (abs(speedControl.forward - forwardValue) > 0.01) ||
                       isZeroState;
    
    if (hasChanged) {
      // 更新速度控制参数
      speedControl.turn = turnValue;
      speedControl.forward = forwardValue;
      speedControl.last_update_time = millis();
      speedControl.data_processed = false; // 标记为未处理
      
      // 打印更新后的速度参数
      if (isZeroState) {
        if (serialConnected) {
          Serial.println("接收到停止命令: 转向=0.00, 前进=0.00");
        }
      } else {
        if (serialConnected) {
          Serial.printf("更新速度参数: 转向=%.2f, 前进=%.2f\n", 
                      speedControl.turn, speedControl.forward);
        }
      }
    }
    
    // 使用静态响应字符串
    static const char* OK_RESPONSE = "OK";
    request->send(200, "text/plain", OK_RESPONSE);
  } else {
    // 使用静态响应字符串
    static const char* ERROR_RESPONSE = "速度值超出有效范围";
    request->send(400, "text/plain", ERROR_RESPONSE);
  }
}

// 处理获取PID参数请求
void handleGetPID(AsyncWebServerRequest *request) {
  StaticJsonDocument<256> doc;
  
  JsonObject pid_stb_json = doc.createNestedObject("pid_stb");
  pid_stb_json["p"] = pid_stb.P;
  pid_stb_json["i"] = pid_stb.I;
  pid_stb_json["d"] = pid_stb.D;
  
  JsonObject pid_vel_json = doc.createNestedObject("pid_vel");
  pid_vel_json["p"] = pid_vel.P;
  pid_vel_json["i"] = pid_vel.I;
  pid_vel_json["d"] = pid_vel.D;
  
  static char jsonBuffer[256];
  size_t len = serializeJson(doc, jsonBuffer, sizeof(jsonBuffer));
  
  if (len > 0 && len < sizeof(jsonBuffer)) {
    request->send(200, "application/json", jsonBuffer);
  } else {
    request->send(500, "application/json", "{\"error\":\"序列化失败\"}");
  }
  
  if (serialConnected) {
    Serial.println("已发送当前PID参数");
  }
}

// 处理设置PID参数请求
void handleSetPID(AsyncWebServerRequest *request, uint8_t *data, size_t len, size_t index, size_t total) {
  if (index + len != total) {
    // 如果数据不完整，等待更多数据
    return;
  }
  
  // 检查数据长度是否合理
  if (len > 512) {
    static const char* SIZE_ERROR = "{\"success\":false,\"message\":\"数据过大\"}";
    request->send(400, "application/json", SIZE_ERROR);
    return;
  }
  
  // 使用静态缓冲区，避免动态内存分配
  static char buffer[512];
  if (len >= sizeof(buffer)) {
    static const char* SIZE_ERROR = "{\"success\":false,\"message\":\"数据过大\"}";
    request->send(400, "application/json", SIZE_ERROR);
    return;
  }
  
  // 复制数据到缓冲区并确保以NULL结尾
  memcpy(buffer, data, len);
  buffer[len] = '\0';
  
  // 使用StaticJsonDocument避免堆内存分配
  StaticJsonDocument<256> doc;
  DeserializationError error = deserializeJson(doc, buffer);
  
  if (error) {
    static const char* JSON_ERROR = "{\"success\":false,\"message\":\"无效的JSON数据\"}";
    request->send(400, "application/json", JSON_ERROR);
    return;
  }
  
  bool valid_data = true;
  static char errorMsg[64] = "";
  
  // 检查是否包含所有必要的PID参数
  if (!doc.containsKey("pid_stb") || !doc.containsKey("pid_vel")) {
    valid_data = false;
    strncpy(errorMsg, "缺少pid_stb或pid_vel参数", sizeof(errorMsg) - 1);
  }
  else {
    // 检查pid_stb参数
    JsonObject pid_stb_json = doc["pid_stb"];
    if (!pid_stb_json.containsKey("p") || !pid_stb_json.containsKey("i") || !pid_stb_json.containsKey("d")) {
      valid_data = false;
      strncpy(errorMsg, "pid_stb缺少p, i或d参数", sizeof(errorMsg) - 1);
    }
    
    // 检查pid_vel参数
    JsonObject pid_vel_json = doc["pid_vel"];
    if (!pid_vel_json.containsKey("p") || !pid_vel_json.containsKey("i") || !pid_vel_json.containsKey("d")) {
      valid_data = false;
      strncpy(errorMsg, "pid_vel缺少p, i或d参数", sizeof(errorMsg) - 1);
    }
  }
  
  if (valid_data) {
    // 更新pid_stb参数
    float p_stb = doc["pid_stb"]["p"];
    float i_stb = doc["pid_stb"]["i"];
    float d_stb = doc["pid_stb"]["d"];
    
    // 更新pid_vel参数
    float p_vel = doc["pid_vel"]["p"];
    float i_vel = doc["pid_vel"]["i"];
    float d_vel = doc["pid_vel"]["d"];
    
    // 应用新的PID参数
    pid_stb.P = p_stb;
    pid_stb.I = i_stb;
    pid_stb.D = d_stb;
    
    pid_vel.P = p_vel;
    pid_vel.I = i_vel;
    pid_vel.D = d_vel;
    
    if (serialConnected) {
      Serial.println("已更新PID参数:");
      Serial.printf("pid_stb: P=%.2f, I=%.2f, D=%.2f\n", p_stb, i_stb, d_stb);
      Serial.printf("pid_vel: P=%.2f, I=%.2f, D=%.2f\n", p_vel, i_vel, d_vel);
    }
    
    // 发送成功响应
    static const char* SUCCESS_RESPONSE = "{\"success\":true}";
    request->send(200, "application/json", SUCCESS_RESPONSE);
  } else {
    // 发送错误响应
    static char response[128];
    snprintf(response, sizeof(response), "{\"success\":false,\"message\":\"%s\"}", errorMsg);
    request->send(400, "application/json", response);
  }
}

void setup() {
  // 串口监视
  Serial.begin(115200);
  
  // 等待最多3秒钟看串口是否连接
  unsigned long startTime = millis();
  while (!Serial && millis() - startTime < 3000) {
    delay(100);
  }
  
  // 检查串口是否已连接
  serialConnected = Serial;
  
  if (serialConnected) {
    Serial.println("串口已连接，启用调试输出");
  }

  // 显示CPU频率信息
  if (serialConnected) {
    Serial.print("CPU频率: ");
    Serial.print(getCpuFrequencyMhz());
    Serial.println(" MHz");
    Serial.print("APB频率: ");
    Serial.print(getApbFrequency() / 1000000);
    Serial.println(" MHz");
  }

  // 初始化WiFi接入点 - 移到前面
  if (serialConnected) {
    Serial.println(F("开始初始化WiFi接入点..."));
  }
  WiFi.softAP(ssid, password);

  IPAddress IP = WiFi.softAPIP();
  if (serialConnected) {
    Serial.print("AP IP地址: ");
    Serial.println(IP);
  }

  // 设置Web服务器路由
  server.on("/", HTTP_GET, [](AsyncWebServerRequest *request){
    request->send_P(200, "text/html", INDEX_HTML);
  });

  // 处理传感器数据请求
  server.on("/sensor-data", HTTP_GET, handleSensorData);

  // 处理控制命令
  server.on("/control", HTTP_POST, handleControl, NULL, handleBody);

  // 处理PID参数请求
  server.on("/get-pid", HTTP_GET, handleGetPID);
  server.on("/set-pid", HTTP_POST, [](AsyncWebServerRequest *request){}, NULL, handleSetPID);

  // 启动服务器
  server.begin();
  if (serialConnected) {
    Serial.println("HTTP服务器已启动");
  }

  // 初始化主I2C通信
  Wire.setPins(17, 18);
  Wire.begin();
  Wire.setClock(100000);
  Wire.setTimeOut(2000); // 增加超时时间到2000ms

  // 检查I2C总线状态
  if (serialConnected && (digitalRead(17) != HIGH || digitalRead(18) != HIGH)) {
    Serial.println("警告: I2C总线1上的SDA或SCL引脚没有被上拉到HIGH状态!");
    Serial.printf("SDA(17)状态: %s, SCL(18)状态: %s\n",
                  digitalRead(17) == HIGH ? "HIGH" : "LOW",
                  digitalRead(18) == HIGH ? "HIGH" : "LOW");
  }
  delay(500); // 增加延迟，确保I2C初始化完成

  // 初始化第二个I2C通信
  Wire1.setPins(8, 9);
  Wire1.begin();
  Wire1.setClock(100000);
  Wire1.setTimeOut(2000); // 增加超时时间到2000ms

  // 检查I2C总线状态
  if (serialConnected && (digitalRead(8) != HIGH || digitalRead(9) != HIGH)) {
    Serial.println("警告: I2C总线2上的SDA或SCL引脚没有被上拉到HIGH状态!");
    Serial.printf("SDA(8)状态: %s, SCL(9)状态: %s\n",
                  digitalRead(8) == HIGH ? "HIGH" : "LOW",
                  digitalRead(9) == HIGH ? "HIGH" : "LOW");
  }
  delay(500); // 增加延迟，确保I2C初始化完成

  // 初始化IMU
  imu.init();
  if (serialConnected) {
    Serial.println(F("IMU初始化完成"));
  }

  // 只在串口连接时启用调试输出
  if (serialConnected) {
    SimpleFOCDebug::enable(&Serial); // 启用调试输出以便排查问题
  }

  // 初始化第一个磁编码器硬件，使用默认Wire
  sensor1.init();

  // 初始化第二个磁编码器硬件，指定使用Wire1
  sensor2.init(&Wire1);

  // 将电机链接到传感器
  motor1.linkSensor(&sensor1);
  motor2.linkSensor(&sensor2);


  // 第一个驱动器配置
  driver1.voltage_power_supply = 12; // 电源电压[V]
  driver1.init();
  motor1.linkDriver(&driver1);

  // 第二个驱动器配置
  driver2.voltage_power_supply = 12; // 电源电压[V]
  driver2.init();
  motor2.linkDriver(&driver2);

  // 设置第一个电机控制模式为扭矩控制
  motor1.torque_controller = TorqueControlType::voltage; // 电压模式扭矩控制
  motor1.controller = MotionControlType::torque;

  // 设置第二个电机控制模式为扭矩控制
  motor2.torque_controller = TorqueControlType::voltage; // 电压模式扭矩控制
  motor2.controller = MotionControlType::torque;

  // 第一个电机控制器参数配置
  motor1.voltage_limit = 12;     // 电压限制
  motor1.LPF_velocity.Tf = 0.07; // 速度低通滤波
  motor1.velocity_limit = 100;   // 最大速度限制
  motor1.PID_velocity.P = 0.05;
  motor1.PID_velocity.I = 0;
  motor1.PID_velocity.D = 0;

  // 第二个电机控制器参数配置
  motor2.voltage_limit = 12;     // 电压限制
  motor2.LPF_velocity.Tf = 0.07; // 速度低通滤波
  motor2.velocity_limit = 100;   // 最大速度限制
  motor2.PID_velocity.P = 0.05;
  motor2.PID_velocity.I = 0;
  motor2.PID_velocity.D = 0;

  // 启用串口监视（可选）
  if (serialConnected) {
    motor1.useMonitoring(Serial);
    motor2.useMonitoring(Serial);
    
    // 大幅减少监视输出以减少串口数据量
    motor1.monitor_downsample = 100; // 只打印每100个数据点中的一个
    motor2.monitor_downsample = 100; // 只打印每100个数据点中的一个
  }

  // 初始化电机
  motor1.init();
  motor2.init();

  // 初始化FOC算法
  motor1.initFOC();
  motor2.initFOC();

  // 设置初始目标值
  motor1.target = 0;
  motor2.target = 0;

  if (serialConnected) {
    Serial.println(F("电机已初始化，使用AS5600编码器"));
  }

  // 初始化打印计时器
  last_print_time = millis();

  _delay(1000);
}

void loop() {
  // 运行FOC算法
  motor1.loopFOC();
  motor2.loopFOC();

  unsigned long current_time = millis();

  // 添加try-catch保护IMU更新过程
  try {
    imu.update();
  } catch (...) {
    if (serialConnected) {
      Serial.println("IMU更新过程发生异常");
    }
  }

  // 尝试读取传感器1数据
  float angle1 = 0;
  // 添加try-catch保护传感器读取
  try {
    angle1 = sensor1.getAngle();
    motor1AngleDeg = angle1 * 180.0f / PI; // 转换为角度
  } catch (...) {
    if (serialConnected) {
      Serial.println("传感器1读取异常");
    }
    motor1AngleDeg = 0;
  }

  // 尝试读取传感器2数据
  float angle2 = 0;
  try {
    angle2 = sensor2.getAngle();
    motor2AngleDeg = angle2 * 180.0f / PI; // 转换为角度
  } catch (...) {
    if (serialConnected) {
      Serial.println("传感器2读取异常");
    }
    motor2AngleDeg = 0;
  }

  //
  motor1SpeedRPM = motor1.shaft_velocity;

  motor2SpeedRPM = motor2.shaft_velocity;

  float offset = 4;

  // 计算目标角度 - 使用固定的PID参数，增加安全检查
  // 先保存电机速度的平均值，避免直接在pid_vel函数中计算
  float avgMotorSpeed = 0.0f;
  if (isnan(motor1SpeedRPM) || isnan(motor2SpeedRPM)) {
    // 如果速度值无效，使用0
    avgMotorSpeed = 0.0f;
    if (serialConnected) {
      Serial.println("警告: 电机速度值无效，使用0");
    }
  } else {
    avgMotorSpeed = -(motor1SpeedRPM + motor2SpeedRPM) / 2.0f;
  }
  
  // 确保forward值在有效范围内
  float safeForward = speedControl.forward;
  if (isnan(safeForward)) {
    safeForward = 0.0f;
    if (serialConnected) {
      Serial.println("警告: forward值无效，使用0");
    }
  }
  
  // 安全调用pid_vel函数
  float pidVelResult = 0.0f;
  try {
    pidVelResult = pid_vel(avgMotorSpeed);
  } catch (...) {
    if (serialConnected) {
      Serial.println("警告: pid_vel计算异常，使用0");
    }
    pidVelResult = 0.0f;
  }
  
  // 计算目标角度
  targetangle = offset + (pidVelResult - safeForward * 10); // 放大前进速度影响
  
  // 限制targetangle在offset±10之间
  if (targetangle < offset - 10) {
    targetangle = offset - 10;
  }
  if (targetangle > offset + 10) {
    targetangle = offset + 10;
  }

  float velocity_control = pid_stb(imu.angle[0] - targetangle);
  
  // 增加转向灵敏度
  float turn_factor = 1.0; // 转向灵敏度系数
  
  motor1.target = velocity_control - speedControl.turn * turn_factor;
  motor1.move();

  motor2.target = velocity_control + speedControl.turn * turn_factor;
  motor2.move();

  // 添加更详细的调试信息
  static unsigned long last_debug_time = 0;
  if (serialConnected && current_time - last_debug_time >= 500) { // 每0.5秒打印一次调试信息
    last_debug_time = current_time;
  }

  // 监控堆内存状态
  static unsigned long last_heap_check = 0;
  if (serialConnected && current_time - last_heap_check >= 10000) { // 每10秒检查一次堆内存
    last_heap_check = current_time;
    
    // 获取并打印堆内存信息
    size_t freeHeap = ESP.getFreeHeap();
    size_t minFreeHeap = ESP.getMinFreeHeap();
    size_t maxAllocHeap = ESP.getMaxAllocHeap();
    
    Serial.println("\n--- 内存状态 ---");
    Serial.printf("可用堆: %u 字节\n", freeHeap);
    Serial.printf("最小可用堆: %u 字节\n", minFreeHeap);
    Serial.printf("最大连续可分配堆: %u 字节\n", maxAllocHeap);
    Serial.printf("堆碎片化程度: %.2f%%\n", 100.0 * (1.0 - ((float)maxAllocHeap / freeHeap)));
  }

  // 检查数据是否已处理
  if (!speedControl.data_processed) {
    // 标记数据已处理
    speedControl.data_processed = true;
  }
  
  // 检查数据是否过期 (超过1秒没有更新)
  if (current_time - speedControl.last_update_time > 1000) {
    // 如果超过1秒没有收到新数据，可以考虑减速或停止
    // 这里只是简单记录，不做实际减速处理
    static unsigned long last_timeout_log = 0;
    if (serialConnected && current_time - last_timeout_log > 5000) { // 每5秒只记录一次
      Serial.println("警告: 超过1秒未收到速度控制数据");
      last_timeout_log = current_time;
    }
  }

  // 每秒发送一次传感器数据
  if (current_time - last_print_time >= print_interval) {
    // 更新打印时间
    last_print_time = current_time;

    print_sensor_data(); // 打印到Serial监视器

    if (serialConnected) {
      Serial.printf("targetangle: %.1f, imu.angle: %.1f\n", targetangle, imu.angle[0]);
    }
  }
}

// 数据打印
void print_sensor_data() {
  // 只在串口连接时输出调试信息
  if (!serialConnected) return;
  
  Serial.println("\n--- Sensor Data (简要) ---");
  Serial.printf("角度: Roll=%.1f° Pitch=%.1f° Yaw=%.1f° | 温度: %.1f°C\n",
                imu.angle[0], imu.angle[1], imu.angle[2], imu.temp);

  // 打印电机角度和转速（简化版）
  Serial.printf(
      "电机1: 角度=%.1f° 速度=%.1f RPM | 电机2: 角度=%.1f° 速度=%.1f RPM\n",
      motor1AngleDeg, motor1SpeedRPM, motor2AngleDeg, motor2SpeedRPM);

  // 打印当前PID参数
  Serial.printf(
      "姿态PID: P=%.2f, I=%.2f, D=%.2f | 补偿角度PID: P=%.2f, I=%.2f, D=%.2f\n",
      pid_stb.P, pid_stb.I, pid_stb.D, pid_vel.P,
      pid_vel.I, pid_vel.D);
      
  // 打印当前速度参数
  Serial.printf("前进速度: %.2f | 转向速度: %.2f\n", 
                speedControl.forward, speedControl.turn);
}